import numpy as np


# 工具函数
def euc_dist(p1, p2):
    return np.sqrt(np.sum((np.array(p1) - np.array(p2)) ** 2))


def comp_dist_mtx(locs):
    n = locs.shape[0]
    mtx = np.zeros((n, n))
    for i in range(n):
        for j in range(n):
            if i != j:
                mtx[i, j] = euc_dist(locs[i], locs[j])
    return mtx


def calc_path_dist(mtx, path):
    dist = 0
    n = len(path)
    for i in range(n):
        dist += mtx[path[i], path[(i + 1) % n]]
    return float(dist)


# 2-opt 优化算法
def two_opt(mtx, path):
    n = len(path)
    improved = True
    while improved:
        improved = False
        for i in range(n):
            for j in range(i + 1, n):
                if (mtx[path[i], path[(i + 1) % n]] + mtx[path[j], path[(j + 1) % n]] >
                        mtx[path[i], path[j]] + mtx[path[(i + 1) % n], path[(j + 1) % n]]):
                    path = path[:i + 1] + path[j:i:-1] + path[j + 1:]
                    improved = True
    return path


# 三点贪心算法
def three_point_greedy(mtx, start=0):
    n = mtx.shape[0]
    visited = [False] * n
    path = [start]
    visited[start] = True

    while len(path) < n:
        remaining_cities = [i for i in range(n) if not visited[i]]
        if len(remaining_cities) <= 2:
            # 如果剩余城市不足两个，直接添加它们到路径中
            path.extend(remaining_cities)
            break
        else:
            min_dist = float('inf')
            next_city = None
            for i in range(n):
                if not visited[i]:
                    for j in range(n):
                        if i != j and not visited[j]:
                            for k in range(n):
                                if i != k and j != k and not visited[k]:
                                    dist = mtx[path[-1], i] + mtx[i, j]
                                    if dist < min_dist:
                                        min_dist = dist
                                        next_city = i
            if next_city is not None:
                path.append(next_city)
                visited[next_city] = True
            else:
                # 如果找不到合适的下一个城市，这可能意味着算法陷入了局部最优解
                # 在这种情况下，我们可以选择一个未访问的城市作为下一个城市
                # 这是一种简单的回退策略，以确保算法能够继续执行
                for city in remaining_cities:
                    path.append(city)
                    visited[city] = True
                    break

    return path


# 三点贪心算法，考虑所有起点，并使用2-opt优化
def three_point_greedy_with_optimization(mtx):
    min_length = float('inf')
    best_path = None

    for start in range(mtx.shape[0]):
        greedy_path = three_point_greedy(mtx, start)
        optimized_path = two_opt(mtx, greedy_path)
        length = calc_path_dist(mtx, optimized_path)

        if length < min_length:
            min_length = length
            best_path = optimized_path

    return best_path, min_length


# 主程序
if __name__ == "__main__":
    # 设置随机种子以确保结果可重复
    np.random.seed(42)

    # 控制位置矩阵的规模
    num_cities = 100

    # 随机生成位置矩阵
    locs = np.random.rand(num_cities, 2) * 100  # 生成0到100之间的随机坐标
    print(locs)

    mtx = comp_dist_mtx(locs)

    # 使用三点贪心算法找到最优路径，并使用2-opt进行优化
    best_path, best_length = three_point_greedy_with_optimization(mtx)

    print("最优三点贪心路径:")
    print(best_path)
    print("总长度:", best_length)
    print(len(best_path))
